﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using log4net;
using System;
using System.Collections.Generic;
using System.Reflection;
using System.Threading;
using System.Timers;
using Safover.LidarCollector.Models;
using Safover.LidarCollector.Config;
using static Safover.LidarCollector.Managers.DeviceManager;
using Safover.LidarCollector.Core;
using Safover.Lidar.DataContract;

namespace Safover.LidarCollector.Managers
{
    public class DataCollector
    {
        private static readonly ILog Log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        private static readonly string Name = "DataCollector";

        internal static LidarConfig LidarConfig => RuntimeConfigManager.Instance.LidarConfig;

        internal static bool ShouldContinueCollection { get; private set; }

        #region Singleton
        public static DataCollector Instance { get; } = new DataCollector();
        private DataCollector()
        {
            Cacher = new DataCacher();
            Cacher.DataCollectorStatusChangedEvent += OnDataCollectorStatusChanged;
        }

        #endregion

        /// <summary>
        /// 采集工作状态改变事件
        /// </summary>
        public GlobalCollectionWorkingStatusChangedEventHandler GlobalCollectionWorkingStatusChangedEvent { get; set; }

        public PeriodicCollectionWorkingStatusChangedEventHandler PeriodicCollectionWorkingStatusChangedEvent { get; set; }

        public SingleCollectionWorkingStatusChangedEventHandler SingleCollectionWorkingStatusChangedEvent { get; set; }

        public MultiStepScanStatusChangedEventHandler MultiStepScanStatusChangedEvent { get; set; }

        /// <summary>
        /// 单次采集超时阈值，单位=秒
        /// 
        /// 规则 = 【采集卡累加次数】除以【激光器频率 x 1000】 + 延时余量
        /// </summary>
        public int SingleAcquisitionTimeout
        {
            get
            {
                var cFrequency = RuntimeConfigManager.Instance.AcquisitionCardConfig.CumulativePulses;
                var laserFrequency = RuntimeConfigManager.Instance.OpticalMaserConfig.Frequency;
                var offset = 15;

                var total = cFrequency / (laserFrequency * 1000) + offset;

                return (int)total;
            }
        }

        public bool IsCollectorThreadWorking { get; private set; }

        private Thread CollectorThread { get; set; }

        private DataCacher Cacher { get; }

        public void StartWorking()
        {
            if (IsCollectorThreadWorking) return;

            CollectorThread = new Thread(CollectionLoop)
            {
                Name = Name,
            };

            var lidarWorkMode = RuntimeConfigManager.Instance.LidarConfig.LidarWorkMode;

            CollectorThread.Start(lidarWorkMode);
        }

        public void StopWorking()
        {
            if (!IsCollectorThreadWorking)
            {
                return;
            }

            _ = DeviceFactoryInstance.OpticalMaserDevice.StopLasar();

            do
            {
                Cacher.StopCache();
                ShouldContinueCollection = false;
            } while (!CollectorThread.Join(1000));
        }

        private void CollectionLoop(object data)
        {
            if (IsCollectorThreadWorking)
            {
                return;
            }

            IsCollectorThreadWorking = true;

            var lidarWorkMode = (LidarWorkMode)data;

            // 通知进入采集状态
            InvokeGlobalCollectionWorkingStatusChangedEvent(lidarWorkMode, IsCollectorThreadWorking);

            try
            {
                SetupAcquisition();

                Log.DebugWriteLine($"{Name}: Collection loop started.");

                ShouldContinueCollection = true;

                if (lidarWorkMode == LidarWorkMode.Multi)
                {
                    MultiStepCollectionLoop();
                }
                else
                {
                    SingleStepCollectionLoop(LidarConfig);
                }
            }
            finally
            {
                IsCollectorThreadWorking = false;

                InvokeGlobalCollectionWorkingStatusChangedEvent(lidarWorkMode, IsCollectorThreadWorking);

                Log.DebugWriteLine($"{Name}: Collection loop returned.");
            }
        }

        private void MultiStepCollectionLoop()
        {
            SingleScanConfig[] scanSteps = LidarConfig.MultiScanSteps.ToArray();
            if (scanSteps.Length <= 0)
            {
                return;
            }

            var currentScanStepIndex = 0;

            while (ShouldContinueCollection)
            {
                SingleScanConfig scanStep = scanSteps[currentScanStepIndex];

                UpdatePeriodicCollectionWorkingStatus(scanStep.LidarWorkMode, true);

                CountDownTimer periodicScanCountDownTimer = StartMultiScanStepCountDownTimer(scanStep.TotalScanDuration);

                while (ShouldContinueCollection && !periodicScanCountDownTimer.IsScanStepTimeout)
                {
                    SingleStepCollectionLoop(LidarConfig, periodicScanCountDownTimer, scanStep);
                }

                StopMultiScanStepCountDownTimer(periodicScanCountDownTimer);

                UpdatePeriodicCollectionWorkingStatus(scanStep.LidarWorkMode, false);

                // 准备执行一个扫描模式
                currentScanStepIndex = (currentScanStepIndex + 1) % scanSteps.Length;
            }

        }

        /// <summary>
        /// 多模式循环扫描下，开启一个单模式运行时间倒计时
        /// </summary>
        /// <returns></returns>
        private CountDownTimer StartMultiScanStepCountDownTimer(TimeSpan periodicScanTotalDuration)
        {
            CountDownTimer countDownTimer = new CountDownTimer(periodicScanTotalDuration);
            countDownTimer.Elapsed += OnMultiScanStepCountDownTimerElapsed;
            countDownTimer.Start();

            return countDownTimer;
        }

        private void StopMultiScanStepCountDownTimer(CountDownTimer countDownTimer)
        {
            countDownTimer.Elapsed -= OnMultiScanStepCountDownTimerElapsed;
            countDownTimer.Stop();
        }

        private void UpdatePeriodicCollectionWorkingStatus(LidarWorkMode lidarWorkMode, bool isPeriodicalCollecting) 
        {
            PeriodicCollectionWorkingStatusChangedEvent?.Invoke(new PeriodicCollectionWorkingStatusChangedEventArgs
            {
                IsPeriodicalCollecting = isPeriodicalCollecting,
                PeriodicaLidarWorkMode = lidarWorkMode
            });
        }

        private void OnMultiScanStepCountDownTimerElapsed(object sender, ElapsedEventArgs e)
        {
            CountDownTimer timer = sender as CountDownTimer;

            UpdateMultiStepScanStatus(true, timer.CountDownSeconds, 0);
        }

        private void UpdateMultiStepScanStatus(bool isMultiStepCollecting, int workingCountDownSeconds, int waitingCountDownSeconds)
        {
            MultiStepScanStatusChangedEvent?.Invoke(new MultiStepScanStatusChangedEvent
            {
                IsMultiStepCollecting = isMultiStepCollecting,

                CountDownSecondsForCurrentMultiStepCollection = workingCountDownSeconds,

                CountDownSecondsForNextMultiStepCollection = waitingCountDownSeconds
            });
        }

        /// <summary>
        /// 单模式采集逻辑
        /// </summary>
        /// <param name="scanParams">采集参数</param>
        /// <param name="singleStepScanCountDownTimer">多模式循环状态下，单次单模式运行时长倒计时</param>
        private void SingleStepCollectionLoop(LidarConfig lidarConfig, CountDownTimer singleStepScanCountDownTimer = null, SingleScanConfig scanStep = null)
        {
            while (ShouldContinueCollection)
            {
                PeriodicScanParams scanParams;
                if (singleStepScanCountDownTimer == null)
                {
                    scanParams = ScanParamsBuilder.GetPeriodicScanParams(lidarConfig);
                }
                else 
                {
                    scanParams = ScanParamsBuilder.GetPeriodicScanParamsForMultiMode(scanStep, LidarConfig);
                }

                Log.DebugWriteLine($"{Environment.NewLine}************* {DateTime.Now:MM-dd HH:mm:ss} ************* Ready to start collection, LidarWorkMode=[{scanParams.LidarWorkMode}].");

                InvokePeriodicCollectionWorkingStatusChangedEvent(scanParams.PeriodicScanLidarWorkMode, true);

                BeforePeriodicCollection(scanParams);

                var function = GetCollectorFunction(scanParams);
                SingleScanResult result = SingleScanResult.Success;

                while (ShouldContinueCollection)
                {
                    result = function.Value();

                    if (SingleScanResult.Success != result)
                    {
                        break;
                    }

                    // 在多模式循环采集状态下，判断单模式是否到期
                    if (singleStepScanCountDownTimer != null && singleStepScanCountDownTimer.IsScanStepTimeout)
                    {// if timeout then break loop and return
                        Log.DebugWriteLine($"{Name}: Step scan timeout, execution timespan=[{singleStepScanCountDownTimer.Stopwatch}].");
                        break;
                    }
                }

                InvokePeriodicCollectionWorkingStatusChangedEvent(scanParams.PeriodicScanLidarWorkMode, false);

                if (singleStepScanCountDownTimer != null && singleStepScanCountDownTimer.IsScanStepTimeout)
                {// 如果设置了采集倒计时，则采集模式为混合模式，混合模式下模式运行时间结束后不需要休息，直接切换

                    SingleCollectionWorkingStatusChangedEvent?.Invoke(new SingleCollectionWorkingStatusChangedEventArgs
                    {
                        IsSingleCollecting = true,
                        SingleLidarWorkMode = scanParams.PeriodicScanLidarWorkMode,
                        PercentageCompletion = 0f,
                        Message = "Idle for next periodical collection",
                        CountDownSecondsForNextFixedCollection = 0,
                        CountDownSecondsForNextPeriodicCollection = 0,
                    });

                    return;
                }

                AfterPeriodicCollection(scanParams as PeriodicRangeScanParams);
            }
        }

        private void AfterPeriodicCollection(PeriodicRangeScanParams scanParams)
        {
            if (null == scanParams)
            {
                return;
            }
            var countDownSeconds = scanParams.PeriodScanEndDelayDuration.TotalSeconds;
            NextPeriodicalCountDown((int)countDownSeconds, scanParams.PeriodicScanLidarWorkMode);
        }

        private void NextPeriodicalCountDown(int countDownSeconds, LidarWorkMode singleLidarWorkMode)
        {
            Log.DebugWriteLine($"{Name}: Periodical collection loop end, delay seconds for next loop is [{countDownSeconds}].");

            for (int i = 0; i <= countDownSeconds; i++)
            {
                if (!ShouldContinueCollection)
                {
                    continue;
                }

                SingleCollectionWorkingStatusChangedEvent?.Invoke(new SingleCollectionWorkingStatusChangedEventArgs
                {
                    IsSingleCollecting = false,
                    SingleLidarWorkMode = singleLidarWorkMode,
                    PercentageCompletion = 0f,
                    Message = "Idle for next periodical collection",
                    CountDownSecondsForNextFixedCollection = 0,
                    CountDownSecondsForNextPeriodicCollection = countDownSeconds - i,
                });

                Thread.Sleep(1000);
            }
        }

        private KeyValuePair<LidarWorkMode, Func<SingleScanResult>> GetCollectorFunction(PeriodicScanParams periodicScanParams)
        {
            Func<SingleScanResult> function = null;

            switch (periodicScanParams.PeriodicScanLidarWorkMode)
            {
                case LidarWorkMode.Fixed:
                    {
                        var scanParams = periodicScanParams as PeriodicFixedScanParams;
                        var delaySeconds = (uint)scanParams.SingleScanEndDelayDuration.TotalSeconds;
                        function = () => { return new FixedScan(periodicScanParams).Scan(delaySeconds, Cacher, SingleCollectionWorkingStatusChangedEvent); };
                    }
                    break;
                case LidarWorkMode.Underway:
                    {
                        function = () => { return new UnderwayScan(periodicScanParams).Scan(
                            Cacher, SingleCollectionWorkingStatusChangedEvent); };
                    }
                    break;
                case LidarWorkMode.Horizontal:
                    {
                        var scanParams = periodicScanParams as PeriodicHorizontalRangeScanParams;
                        var startAngle = scanParams.HorizontalStartAngle;
                        var rangeAngle = scanParams.RangeAngle;
                        var stepAngle = scanParams.StepAngle;
                        var fixedVerticalAngle = scanParams.FixedAngle;
                        var blindSectors = scanParams.BlindSectorConfigs;
                        var controlLogic = new HorizontalScanControlLogic(startAngle, rangeAngle, stepAngle, fixedVerticalAngle, blindSectors);

                        function = () => { return new HorizontalScan(periodicScanParams).Scan(controlLogic, /*folderName,*/ Cacher, SingleCollectionWorkingStatusChangedEvent); };
                    }
                    break;
                case LidarWorkMode.Vertical:
                    {
                        var scanParams = periodicScanParams as PeriodicVerticalRangeScanParams;
                        var startAngle = scanParams.VerticalStartAngle;
                        var rangeAngle = scanParams.RangeAngle;
                        var stepAngle = scanParams.StepAngle;
                        var fixedHorizontalAngle = scanParams.FixedAngle;
                        var blindSectors = scanParams.BlindSectorConfigs;
                        var controlLogic = new VerticalScanControlLogic(startAngle, rangeAngle, stepAngle, fixedHorizontalAngle, blindSectors);

                        function = () => { return new VerticalScan(periodicScanParams).Scan(controlLogic, /*folderName,*/ Cacher, SingleCollectionWorkingStatusChangedEvent); };
                    }
                    break;
                
                case LidarWorkMode.Multi:
                default:
                    throw new SafoverConfigurationErrorException($"Configuration error, cannot build control logic for LidarWorkMode=[{periodicScanParams.LidarWorkMode}]");
            }

            return new KeyValuePair<LidarWorkMode, Func<SingleScanResult>>(periodicScanParams.PeriodicScanLidarWorkMode, function);
        }

        /// <summary>
        /// 周期性采集开始前：
        /// 1. 将云台移动到采集周期开始前的初始位置
        /// </summary>
        /// <param name="lidarWorkMode">采集模式</param>
        private void BeforePeriodicCollection(PeriodicScanParams periodicScanParams)
        {
            float verticalAngle, horizontalAngle;
            switch (periodicScanParams.PeriodicScanLidarWorkMode)
            {
               
                case LidarWorkMode.Horizontal:
                    {
                        var scanParams = periodicScanParams as PeriodicHorizontalRangeScanParams;
                        verticalAngle = scanParams.FixedAngle;
                        horizontalAngle = scanParams.HorizontalStartAngle;
                    }
                    break;
                case LidarWorkMode.Vertical:
                    {
                        var scanParams = periodicScanParams as PeriodicVerticalRangeScanParams;
                        verticalAngle = scanParams.VerticalStartAngle;
                        horizontalAngle = scanParams.FixedAngle;
                    }
                    break;
                case LidarWorkMode.Fixed:
                case LidarWorkMode.Underway:
                    {
                        var scanParams = periodicScanParams as PeriodicUnmovableScanParams;
                        verticalAngle = scanParams.FixedVerticalAngle;
                        horizontalAngle = scanParams.FixedHorizontalAngle;
                    }
                    break;
                case LidarWorkMode.Multi:
                default:
                    return;
            }

            if (MoveRotaryTableVertically(verticalAngle))
            {
                Log.DebugWriteLine($"{Name}: Before periodic [{periodicScanParams.PeriodicScanLidarWorkMode}]: Rotary Table has been moved vertically to , vertical angle = [{verticalAngle}].");
            }

            Thread.Sleep(1000);

            if (MoveRotaryTableHorizontally(horizontalAngle))
            {
                Log.DebugWriteLine($"{Name}: Before periodic [{periodicScanParams.PeriodicScanLidarWorkMode}]: Rotary Table has been moved horizontially to ,  horizontial angle = [{horizontalAngle}].");
            }
        }

        private void SetupAcquisition() 
        {
            DeviceFactoryInstance.OpticalMaserDevice.StopLasar();

            DeviceFactoryInstance.AcquisitionCard.Reset();

            if (!DeviceFactoryInstance.AcquisitionCard.StartAcquisition())
            {
                throw new AcquisitionCardMalfunctionException("Failed to start acquisition.");
            }

            Thread.Sleep(1000);
        }

        private void OnDataCollectorStatusChanged(SingleCollectionWorkingStatusChangedEventArgs e)
        {
            SingleCollectionWorkingStatusChangedEvent?.Invoke(e);
        }

        private void InvokeGlobalCollectionWorkingStatusChangedEvent(LidarWorkMode lidarWorkMode, bool isCollecting)
        {
            try
            {
                GlobalCollectionWorkingStatusChangedEvent?.Invoke(new GlobalCollectionWorkingStatusChangedEventArgs
                {
                    IsCollecting = isCollecting,
                    LidarWorkMode = lidarWorkMode
                });
            }
            catch (Exception e)
            {
                Log.Error(e);
            }
        }

        private void InvokePeriodicCollectionWorkingStatusChangedEvent(LidarWorkMode lidarWorkMode, bool isPreiodicalCollecting) 
        {
            try
            {
                PeriodicCollectionWorkingStatusChangedEvent?.Invoke(new PeriodicCollectionWorkingStatusChangedEventArgs
                {
                    IsPeriodicalCollecting = isPreiodicalCollecting,
                    PeriodicaLidarWorkMode = lidarWorkMode
                });
            }
            catch (Exception e)
            {
                Log.Error(e);
            }
        }

        private bool MoveRotaryTableHorizontally(float horizontalAngle)
        {
            try
            {
                return DeviceFactoryInstance.RotaryTableDevice.MoveHorizontallyTo(horizontalAngle);
            }
            catch (Exception e)
            {
                throw new RotaryTableControlException("Failed Moving Rotary Table Horizontally.", e);
            }
        }

        private bool MoveRotaryTableVertically(float verticalAngle)
        {
            try
            {
                return DeviceFactoryInstance.RotaryTableDevice.MoveVerticallyTo(verticalAngle);
            }
            catch (Exception e)
            {
                throw new RotaryTableControlException("Failed Moving Rotary Table Vertically.", e);
            }
        }
    }

}
